#include "cartographer/rcm/imu/complementary_wrap.h"

namespace cartographer
{
    ComplementaryFilterWrap::ComplementaryFilterWrap()
    {
    }

    bool ComplementaryFilterWrap::Initiate()
    {
        auto &options = ImuOptionsComplementary::GetInstance();
        filter_.setDoAdaptiveGain(options.do_adaptive_gain);
        filter_.setDoBiasEstimation(options.do_bias_estimation);

        if (!filter_.setGainAcc(options.gain_acc))
            LOG(INFO) << "Invalid gain_acc passed to ComplementaryFilter.";
        if (options.use_mag)
        {
            if (!filter_.setGainMag(options.gain_mag))
                LOG(INFO) << "Invalid gain_mag passed to ComplementaryFilter.";
        }
        if (options.do_bias_estimation)
        {
            if (!filter_.setBiasAlpha(options.bias_alpha))
                LOG(INFO) << "Invalid bias_alpha passed to ComplementaryFilter.";
        }
        return true;
    }

    bool ComplementaryFilterWrap::Update(const sensor::ImuData &imu_raw)
    {
        const auto &acceleration = imu_raw.linear_acceleration;
        const auto &angular = imu_raw.angular_velocity;
        const auto &time = imu_raw.time;
        if (!initialized_)
        {
            time_prev_ = time;
            initialized_ = true;
            return false;
        }
        double dt = common::ToSeconds(time - time_prev_);
        filter_.update(acceleration, angular, dt);
        return true;
    }

    void ComplementaryFilterWrap::GetFilteredImu(sensor::ImuData &imu_raw)
    {
        double q0, q1, q2, q3;
        filter_.getOrientation(q0, q1, q2, q3);

        if (filter_.getDoBiasEstimation())
        {
            imu_raw.angular_velocity(0) -= filter_.getAngularVelocityBiasX();
            imu_raw.angular_velocity(1) -= filter_.getAngularVelocityBiasY();
            imu_raw.angular_velocity(2) -= filter_.getAngularVelocityBiasZ();
        }
    }
} // namespace cartographer